#include <Windows.h>
#include <SDL.h>
#include <gl\GL.h>
#include <gl\GLU.h>
#include <vector>
#include "Helper.h"
#include "btBulletDynamicsCommon.h"
#include "Vehicle.h"
#include "DebugDrawer.h"
#include "Camera.h"
#include "KeyManager.h"

btDynamicsWorld* simWorld;
btDispatcher* dispatcher;
btBroadphaseInterface* broadPhase;
btDefaultCollisionConfiguration* configuration;
btSequentialImpulseConstraintSolver* solver;
btIDebugDraw* debugDrawer;
Camera cam;
Vehicle* car;

std::vector<btRigidBody *> bodies;
GLUquadricObj* quad;

float engineForce = 0;
float breakForce = 0;

void init()
{
	SDL_Init( SDL_INIT_EVERYTHING );
	SDL_Surface * screen = SDL_SetVideoMode(480, 320, 32, SDL_SWSURFACE | SDL_OPENGL);

	glClearColor(0.7f, 0.7f, 0.7f, 1.0);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(45, 640.0/480.0, 1.0, 500.0);
	glMatrixMode(GL_MODELVIEW);
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_FOG);
	glFogi(GL_FOG_MODE, GL_LINEAR);
	glFogf(GL_FOG_START, 5.f);
	glFogf(GL_FOG_END, 50.f);
	float fogColor[] = {0.7f, 0.7f, 0.7f};
	glFogfv(GL_FOG_COLOR, fogColor);

	quad = gluNewQuadric();

	configuration = new btDefaultCollisionConfiguration();
	dispatcher = new btCollisionDispatcher(configuration);
	broadPhase = new btDbvtBroadphase();
	solver = new btSequentialImpulseConstraintSolver();
	
	simWorld = new btDiscreteDynamicsWorld(dispatcher,broadPhase,solver,configuration);
	simWorld->setGravity(btVector3(0,-10,0));
	
	debugDrawer = new DebugDrawer();
	debugDrawer->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
	simWorld->setDebugDrawer(debugDrawer);

	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),1);
		
	btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0)));
    btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
    btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
    simWorld->addRigidBody(groundRigidBody);

	VehicleDescription desc;
	desc.mVehicleInitialPosition = btVector3(0, 2, -10);
	desc.mVehicleBodySize = btVector3(1.6f, 1.f, 3.f);
	desc.mVehicleCoordinateSystem = btVector3(0, 1, 2);
	desc.mWheelDirection = btVector3(0, -1, 0);
	desc.mWheelAxle = btVector3(-1, 0, 0);
	desc.mFrontLeftWheelOffset = btVector3(-0.8f, 0.0, 1.5f);
	desc.mFrontRightWheelOffset = btVector3(0.8f, 0.0, 1.5f);
	desc.mRearLeftWheelOffset = btVector3(-0.8f, 0.0, -1.5f);
	desc.mRearRightWheelOffset = btVector3(0.8f, 0.0, -1.5f);
	desc.mVehicleMass = 800.f;
	desc.mSuspensionStiffness = 15.f;
	desc.mSuspensionRestLength = 0.6f;
	desc.mWheelsDampingRelaxation = 2.3f;
	desc.mWheelsDampingCompression = 4.4f;
	desc.mWheelFriction = 1e30f;
	desc.mWheelRadius = 0.8f;
	desc.mFrictionSlip = 10.5f;
	desc.mRollInfluence = 0.1f;
	desc.mMaxEngineForce = 2000.f;
	desc.mMaxBreakingForce = 100.f;
	desc.mMaxSteeringAngle = 0.4f;

	car = new Vehicle(desc, simWorld, bodies);
}

void dropBox()
{
	btCollisionShape* boxShape = new btBoxShape(btVector3(3,0.2,3));

	btScalar mass = 1.0f;
	btVector3 localInertia(0,0,0);

	boxShape->calculateLocalInertia(mass,localInertia);

	btTransform startTransform;
	startTransform.setIdentity();
	startTransform.setOrigin(btVector3(5,5,0));

	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,boxShape,localInertia);
	btRigidBody * body = new btRigidBody(rbInfo);
	bodies.push_back(body);

	simWorld->addRigidBody(body);
}
#include <iostream>
void render()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glLoadIdentity();

	//cam.update();
	gluLookAt(5.f, 10.f, -20.f, 0, 0, 0, 0, 1.f, 0);
	drawFloor(-1.f, 1.f, 50);

	/*std::vector<btRigidBody *>::iterator it = bodies.begin();
	for (; it != bodies.end(); ++it) {
		btRigidBody * body = *it;
		float m[16];
		btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
		myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);

		glPushMatrix();
		glMultMatrixf(m);
		btBoxShape* shape = (btBoxShape*)body->getCollisionShape();
		btVector3 vec = shape->getHalfExtentsWithoutMargin();
		drawBox(vec.x()*2, vec.y()*2, vec.z()*2);
		glPopMatrix();
	}*/
	float mat[16];
	for (int i=0; i < car->getWheelCount(); ++i) {
		float r = car->getWheelMatrix(mat, i);
		renderCylinder(quad, mat, r);
	}

	simWorld->debugDrawWorld();
	DebugDrawer * myDrawer = (DebugDrawer*)debugDrawer;
	myDrawer->drawAll();
	myDrawer->clear();
}

int main(int argc, char** argv)
{
	init();

	bool running = true;
	Uint32 start;
	SDL_Event evt;

	while (running)
	{
		start = SDL_GetTicks();
		while (SDL_PollEvent(&evt))
		{
			switch(evt.type)
			{
			case SDL_QUIT:
				running = false;
				break;
			case SDL_KEYDOWN:
				if (evt.key.keysym.sym == SDLK_ESCAPE) {
					running = false;
				}
				if (evt.key.keysym.sym == SDLK_p) {
					dropBox();
				}
				KeyManager::get()->keyPressed(evt.key.keysym.sym);
				break;
			case SDL_KEYUP:
				KeyManager::get()->keyReleased(evt.key.keysym.sym);
				break;
			}
		}
		car->drive();
		simWorld->stepSimulation(1.0f/60.0f);
		render();
		SDL_GL_SwapBuffers();

		if (1000/30 > (SDL_GetTicks() - start))
			SDL_Delay(1000/30 - (SDL_GetTicks() - start));
	}

	SDL_Quit();

	return 0;
}